#include "Tools.h"

#include <cmath>

using namespace SG::Algebra;

namespace SG::Algebra
{
    /**
     * @brief    计算平面三角形面积。并返回法线方向
     * 
     * @param    vertices 平面三角形坐标，维度3*2 [x1 y1; x2, y2; x3, y3]
     * @param    normal 
     * @return   
     * @author   wanghuan
     * @date     2024.12.29
     */
    Real_t calc2DTriArea (const Matrix& vertices, Vector3D& _OUT normal)
    {
        const Vector3D vectorAB{vertices(1, 0) - vertices(0, 0),  vertices(1, 1) - vertices(0, 1), 0.0};
        const Vector3D vectorAC{vertices(2, 0) - vertices(0, 0),  vertices(2, 1) - vertices(0, 1), 0.0};
        normal            = Cross (vectorAB, vectorAC);
        Real_t norm;
        normal.Normalize (norm);
        return std::fabs (norm / 2);
    }
    
    Real_t calcTriArea (const Point& point1, const Point& point2, const Point& point3, Vector3D& _OUT normal)
    {
        const Vector3D vectorAB = Distance (point2, point1);
        const Vector3D vectorAC = Distance (point3, point1);
        normal            = Cross (vectorAB, vectorAC);
        Real_t norm;
        normal.Normalize (norm);
        return std::fabs (norm / 2);
    }

    Real_t calcTriArea (const Matrix& vertices, Vector3D& _OUT normal)
    {
        const Point point1{vertices (0, 0), vertices (0, 1), vertices (0, 2)};
        const Point point2{vertices (1, 0), vertices (1, 1), vertices (1, 2)};
        const Point point3{vertices (2, 0), vertices (2, 1), vertices (2, 2)};

        const auto area = calcTriArea (point1, point2, point3, normal);
        return area;
    }

    Real_t calcTetraVolumn (const Matrix& vertices)
    {
        Matrix augmentedVertices (4, 4);
        const Matrix ones (4, 1, 1.0);
        augmentedVertices.BlockFill (0, 0, vertices);
        augmentedVertices.BlockFill (0, 3, ones);

        Real_t volumn;
        volumn = std::fabs (det4X4 (augmentedVertices)) / 6.0;

        return volumn;
    }

    Real_t calcTetraVolumn (const Point& point1, const Point& point2, const Point& point3, const Point& point4)
    {
        Matrix augmentedVertices (4, 4, 1.0);
        augmentedVertices (0, 1) = point1.m_x;
        augmentedVertices (0, 2) = point1.m_y;
        augmentedVertices (0, 3) = point1.m_z;
        augmentedVertices (1, 1) = point2.m_x;
        augmentedVertices (1, 2) = point2.m_y;
        augmentedVertices (1, 3) = point2.m_z;
        augmentedVertices (2, 1) = point3.m_x;
        augmentedVertices (2, 2) = point3.m_y;
        augmentedVertices (2, 3) = point3.m_z;
        augmentedVertices (3, 1) = point4.m_x;
        augmentedVertices (3, 2) = point4.m_y;
        augmentedVertices (3, 3) = point4.m_z;
        return calcTetraVolumn (augmentedVertices);
    }

}  // namespace SG::Algebra